import os
import time
from multiprocessing import Process
import threading
global timer
import cv2 as cv
import numpy as np
import vtk
from PIL import ImageQt, Image
from PyQt5 import QtCore, QtGui, QtWidgets
from PyQt5.QtGui import QImage, QPixmap, QTextCursor
from matplotlib import pyplot as plt
from pyqtgraph.opengl import GLViewWidget, GLMeshItem
from traits.trait_types import self
from vtkmodules.qt.QVTKRenderWindowInteractor import QVTKRenderWindowInteractor
import open3d as o3d

import camera
import sfm
from sfm import *
from slam import Ui_MainWindow
import pyqtgraph.opengl as gl
import sys
import math

from PyQt5.QtCore import pyqtSignal, QPoint, QSize, Qt, QThread, QTimer
from PyQt5.QtGui import QColor
from PyQt5.QtWidgets import (QApplication, QHBoxLayout, QOpenGLWidget, QSlider,
                             QWidget, QFileDialog, QTextEdit)
class MyThread(QThread):
    signalForText = pyqtSignal(str)

    def __init__(self, data=None, parent=None):
        super(MyThread, self).__init__(parent)
        self.data = data

    def write(self, text):
        self.signalForText.emit(str(text))  # 发射信号
##声明一个无参数的信号
sin1 = pyqtSignal()


def run(name):
    print("子进程ID号:%d，run：%s" % (os.getpid(), name))  # os.getpid()进程ID
class GLWidget(QOpenGLWidget):
    xRotationChanged = pyqtSignal(int)
    yRotationChanged = pyqtSignal(int)
    zRotationChanged = pyqtSignal(int)

    def __init__(self, parent=None):
        super(GLWidget, self).__init__(parent)

        self.object = 0
        self.xRot = 0
        self.yRot = 0
        self.zRot = 0

        self.lastPos = QPoint()

        self.trolltechGreen = QColor.fromCmykF(0.40, 0.0, 1.0, 0.0)
        self.trolltechPurple = QColor.fromCmykF(0.39, 0.39, 0.0, 0.0)

    def getOpenglInfo(self):
        info = """
            Vendor: {0}
            Renderer: {1}
            OpenGL Version: {2}
            Shader Version: {3}
        """.format(
            gl.glGetString(gl.GL_VENDOR),
            gl.glGetString(gl.GL_RENDERER),
            gl.glGetString(gl.GL_VERSION),
            gl.glGetString(gl.GL_SHADING_LANGUAGE_VERSION)
        )

        return info

    def minimumSizeHint(self):
        return QSize(50, 50)

    def sizeHint(self):
        return QSize(400, 400)

    def setXRotation(self, angle):
        angle = self.normalizeAngle(angle)
        if angle != self.xRot:
            self.xRot = angle
            self.xRotationChanged.emit(angle)
            self.update()

    def setYRotation(self, angle):
        angle = self.normalizeAngle(angle)
        if angle != self.yRot:
            self.yRot = angle
            self.yRotationChanged.emit(angle)
            self.update()

    def setZRotation(self, angle):
        angle = self.normalizeAngle(angle)
        if angle != self.zRot:
            self.zRot = angle
            self.zRotationChanged.emit(angle)
            self.update()

    def initializeGL(self):
        print(self.getOpenglInfo())

        self.setClearColor(self.trolltechPurple.darker())
        self.object = self.makeObject()
        print(np.asarray(self.object))
        gl.glShadeModel(gl.GL_FLAT)
        gl.glEnable(gl.GL_DEPTH_TEST)
        gl.glEnable(gl.GL_CULL_FACE)

    def paintGL(self):
        gl.glClear(
            gl.GL_COLOR_BUFFER_BIT | gl.GL_DEPTH_BUFFER_BIT)
        gl.glLoadIdentity()
        gl.glTranslated(0.0, 0.0, -10.0)
        gl.glRotated(self.xRot / 16.0, 1.0, 0.0, 0.0)
        gl.glRotated(self.yRot / 16.0, 0.0, 1.0, 0.0)
        gl.glRotated(self.zRot / 16.0, 0.0, 0.0, 1.0)
        gl.glCallList(self.object)

    def resizeGL(self, width, height):
        side = min(width, height)
        if side < 0:
            return

        gl.glViewport((width - side) // 2, (height - side) // 2, side,
                           side)

        gl.glMatrixMode(gl.GL_PROJECTION)
        gl.glLoadIdentity()
        gl.glOrtho(-0.5, +0.5, +0.5, -0.5, 4.0, 15.0)
        gl.glMatrixMode(gl.GL_MODELVIEW)

    def mousePressEvent(self, event):
        self.lastPos = event.pos()

    def mouseMoveEvent(self, event):
        dx = event.x() - self.lastPos.x()
        dy = event.y() - self.lastPos.y()

        if event.buttons() & Qt.LeftButton:
            self.setXRotation(self.xRot + 8 * dy)
            self.setYRotation(self.yRot + 8 * dx)
        elif event.buttons() & Qt.RightButton:
            self.setXRotation(self.xRot + 8 * dy)
            self.setZRotation(self.zRot + 8 * dx)

        self.lastPos = event.pos()

    def makeObject(self):
        genList = gl.glGenLists(1)
        gl.glNewList(genList, gl.GL_COMPILE)

        gl.glBegin(gl.GL_QUADS)

        x1 = +0.06
        y1 = -0.14
        x2 = +0.14
        y2 = -0.06
        x3 = +0.08
        y3 = +0.00
        x4 = +0.30
        y4 = +0.22

        self.quad(x1, y1, x2, y2, y2, x2, y1, x1)
        self.quad(x3, y3, x4, y4, y4, x4, y3, x3)

        self.extrude(x1, y1, x2, y2)
        self.extrude(x2, y2, y2, x2)
        self.extrude(y2, x2, y1, x1)
        self.extrude(y1, x1, x1, y1)
        self.extrude(x3, y3, x4, y4)
        self.extrude(x4, y4, y4, x4)
        self.extrude(y4, x4, y3, x3)

        NumSectors = 200

        for i in range(NumSectors):
            angle1 = (i * 2 * math.pi) / NumSectors
            x5 = 0.30 * math.sin(angle1)
            y5 = 0.30 * math.cos(angle1)
            x6 = 0.20 * math.sin(angle1)
            y6 = 0.20 * math.cos(angle1)

            angle2 = ((i + 1) * 2 * math.pi) / NumSectors
            x7 = 0.20 * math.sin(angle2)
            y7 = 0.20 * math.cos(angle2)
            x8 = 0.30 * math.sin(angle2)
            y8 = 0.30 * math.cos(angle2)

            self.quad(x5, y5, x6, y6, x7, y7, x8, y8)

            self.extrude(x6, y6, x7, y7)
            self.extrude(x8, y8, x5, y5)

        gl.glEnd()
        gl.glEndList()

        return genList

    def quad(self, x1, y1, x2, y2, x3, y3, x4, y4):
        self.setColor(self.trolltechGreen)

        gl.glVertex3d(x1, y1, -0.05)
        gl.glVertex3d(x2, y2, -0.05)
        gl.glVertex3d(x3, y3, -0.05)
        gl.glVertex3d(x4, y4, -0.05)

        gl.glVertex3d(x4, y4, +0.05)
        gl.glVertex3d(x3, y3, +0.05)
        gl.glVertex3d(x2, y2, +0.05)
        gl.glVertex3d(x1, y1, +0.05)

    def extrude(self, x1, y1, x2, y2):
        self.setColor(self.trolltechGreen.darker(250 + int(100 * x1)))

        gl.glVertex3d(x1, y1, +0.05)
        gl.glVertex3d(x2, y2, +0.05)
        gl.glVertex3d(x2, y2, -0.05)
        gl.glVertex3d(x1, y1, -0.05)

    def normalizeAngle(self, angle):
        while angle < 0:
            angle += 360 * 16
        while angle > 360 * 16:
            angle -= 360 * 16
        return angle

    def setClearColor(self, c):
        gl.glClearColor(c.redF(), c.greenF(), c.blueF(), c.alphaF())

    def setColor(self, c):
        gl.glColor4f(c.redF(), c.greenF(), c.blueF(), c.alphaF())

class mywindow(QtWidgets.QMainWindow, Ui_MainWindow):
    def __init__(self):
        super(mywindow, self).__init__()
        self.setupUi(self)
        self.setFocusPolicy(Qt.StrongFocus)
        self.timer_pointcloud_mutil_pointcloud = QTimer(self)
        self.test_process = QTimer(self)

        self.timer_pointcloud_mutil_pointcloud.timeout.connect(self.pointcloud_mutil_pointcloud_update)
        self.timer_pointcloud_mutil_pointcloud.start(50)  # 设置时间间隔 50 ms
        # 点云显示控件
        self.glMeshItem = GLMeshItem()
        self.graphicsView = GLViewWidget_1(self)
        self.graphicsView.opts['distance'] = 20
        # self.verticalLayout_2.addWidget(self.glMeshItem)
        self.verticalLayout_2.addWidget(self.graphicsView)
        self.show()
        self.selectPointCloud.clicked.connect(self.play_pointclouds_demo)  # 槽函数不用加括号
        self.pushButton.clicked.connect(self.selectData)  # 槽函数不用加括号
        self.th = MyThread()
        self.th.signalForText.connect(self.onUpdateText)

        # self.fps = QTextEdit(self)
        # self.fps.setText("test open")
        # self.fps.setTextColor(QColor(0, 23, 0))
        # self.fps.setStyleSheet("background:rgb(0, 0, 0);color: rgb(255, 255, 255)")
        # self.verticalLayout_2.addItem(self.fps)

        #输出到ui界面
        # sys.stdout = self.th
        self.recommend_layout = QtWidgets.QGridLayout()
        self.process = QTextEdit(self)
        self.origin_center = []
        self.process.setTextColor(QColor(0,23,0))
        self.process.setStyleSheet("background:rgb(0, 0, 0);color: rgb(255, 255, 255)")
        self.process.ensureCursorVisible()
        self.process.setLineWrapColumnOrWidth(800)
        self.process.setLineWrapMode(QTextEdit.FixedPixelWidth)
        self.process.move(30, 50)
        self.recommend_layout = QtWidgets.QGridLayout()
        self.recommend_widget = QtWidgets.QWidget()
        self.recommend_widget.setLayout(self.recommend_layout)
        self.recommend_layout.addWidget(self.process, 0, 1)
        self.verticalLayout_3.addWidget(self.recommend_widget)
        # self.actionblack.triggered.connect(self.actionblack)
        # self.actionwhite.triggered.connect(self.actionwhite)
        gl_glgrideitem = gl.GLGridItem( size=QtGui.QVector3D(50,50,1))  # 添加网格
        self.graphicsView.addItem(gl_glgrideitem)
        gl_axis = gl.GLAxisItem(size=QtGui.QVector3D(10,10,10))  # 添加xyz坐标轴
        self.graphicsView.addItem(gl_axis)
        #3d点云
        self.sp2 = gl.GLScatterPlotItem(pos=None, color=(1, 1, 1, 1))  # 不带有任何颜色的白点
        #相机参数
        self.sp_camera = gl.GLScatterPlotItem(pos=None, color=(255,255,0, 1))
        phase = 0.
        self.graphicsView.addItem(self.sp2)
        #相机路径
        self.point_slam = gl.GLLinePlotItem(
            pos=None,
            width=3,
            color=(1.0, 1.0, 1.0, 1.0),
            mode='line_strip')

        self.graphicsView.addItem(self.point_slam)
        # 设置包围框，可多个
        # self.c_name_list = []
        # for i in range(100):
        #     c_name_var = "c_name" + "_" + str(i)
        #     self.c_name_list.append(c_name_var)
        #     # print("i", i, len(self.c_name_list))
        # for j in range(0,100):
        #     # print("j", j, self.c_name_list[5])
        #     self.c_name_list[j] = gl.GLBoxItem()
        #     self.c_name_list[j].setColor(0,255,0)
        #     self.c_name_list[j].setSize(float(4.5), float(1), float(1))
        #     self.graphicsView.addItem(self.c_name_list[j])
        self.sp_camera=camera.set_camera()
        # self.w_gl_item = gl.GLLinePlotItem(
        #     pos=self.sp_camera,
        #     width=10,
        #     color=(1.0, 1.0, 0.0, 1.0),
        #     mode='line_strip')
        #
        # self.graphicsView.addItem(self.w_gl_item)
    # 无法获取到GLViewWidget焦点 待改进
    def keyPressEvent(self, QKeyEvent):
        # 这里event.key（）显示的是按键的编码
        print("按下：" + str(QKeyEvent.key()))
        # 举例，这里Qt.Key_A注意虽然字母大写，但按键事件对大小写不敏感
        if (QKeyEvent.key() == Qt.Key_Escape):
            print('测试：ESC')
        if (QKeyEvent.key() == Qt.Key_W):

            rotation_matrix_z = np.asarray([[math.cos(0.1), math.sin(0.1), 0], [-math.sin(0.1), math.cos(0.1), 0], [0, 0,1]])
            self.sp_camera=np.matmul(self.sp_camera, rotation_matrix_z)
            self.w_gl_item.setData(   pos=self.sp_camera,
            width=10,
            color=(1.0, 1.0, 0.0, 1.0),
            mode='line_strip')

        if (QKeyEvent.key() == Qt.Key_Q):
            rotation_matrix_x = np.asarray([[1, 0, 0], [0, math.cos(0.1), math.sin(0.1)], [0, -math.sin(0.1), math.cos(0.1)]])
            self.sp_camera = np.matmul(self.sp_camera, rotation_matrix_x)
            self.w_gl_item.setData(pos=self.sp_camera,
                                   width=10,
                                   color=(1.0, 1.0, 0.0, 1.0),
                                   mode='line_strip')
        if (QKeyEvent.key() == Qt.Key_E):
            rotation_matrix_y = np.asarray(
                [[math.cos(0.1), 0, -math.sin(0.1)], [0, 1, 0], [math.sin(0.1), 0, math.cos(0.1)]])
            self.sp_camera = np.matmul(self.sp_camera, rotation_matrix_y)
            self.w_gl_item.setData(pos=self.sp_camera,
                                   width=10,
                                   color=(1.0, 1.0, 0.0, 1.0),
                                   mode='line_strip')
        #测试相机轨迹 完成
        if (QKeyEvent.key() == Qt.Key_1):
            # sfm.main()
            self.test_multiprocess()
    def test_multiprocess(self):
        self.origin_center = []
        b = np.loadtxt('./KeyFrameTrajectory.txt')
        self.test_process.timeout.connect(self.call)
        self.test_process.start(50)  # 设置时间间隔 50 ms
        for i in b:
            self.origin_center.append(np.asarray([i[1], i[2], i[3]], dtype="float32"))
        self.test_process.timeout.connect(self.call)
        self.test_process.start(50)  # 设置时间间隔 50 ms


    #定时调用更新
    def call(self):
        #
        self.point_slam.setData(
            pos=np.asarray(self.origin_center),
            width=3,
            color=(1.0, 1.0, 1.0, 1.0),
            mode='line_strip')

    def test1(self):
        b = np.loadtxt('./CameraTrajectory.txt', dtype=np.float32)
        b_camera = np.loadtxt('./KeyFrameTrajectory.txt', dtype=np.float32)
        # 相机轨迹
        origin_center = []
        # 初始位置
        origin_center.append(np.asarray([b[0][1], b[0][2], b[0][3]], dtype="float32"))
        # 相机位姿
        camera_data = []
        # 初始姿态
        self.sp_camera = camera.update_camera_position(center=np.asarray([0,0,0], dtype="float32"),size=0.05)
        camera_data.append(self.sp_camera)
        # origin_center.append(self.sp_camera[2])
        self.point_slam = gl.GLLinePlotItem(
            pos=origin_center,
            width=3,
            color=(1.0, 1.0, 1.0, 1.0),
            mode='line_strip')

        self.graphicsView.addItem(self.point_slam)
        print(b.shape)
        t_time = 0
        for i in b_camera:
            # 相机姿态 四元组转化为旋转矩阵
            rot_matrix = np.asarray([[1 - 2 * i[6] * i[6] - 2 * i[7] * i[7], 2 * i[5] * i[6] + 2 * i[4] * i[7],
                                      2 * i[5] * i[7] - 2 * i[4] * i[6]],
                                     [2 * i[5] * i[6] - 2 * i[4] * i[7], 1 - 2 * i[5] * i[5] - 2 * i[7] * i[7],
                                      2 * i[6] * i[7] + 2 * i[4] * i[5]],
                                     [2 * i[5] * i[7] + 2 * i[4] * i[6], 2 * i[6] * i[7] - 2 * i[4] * i[5],
                                      1 - 2 * i[5] * i[5] - 2 * i[6] * i[6]]
                                     ], dtype="float32")
            t_matrix = np.asarray([i[4], i[5], i[6]], dtype="float32")
            print("sp_camera:", self.sp_camera)
            # print("R：", R_matrix)
            print("t：", t_matrix)
            print(origin_center[-1] + t_matrix)
            # 相机位置
            origin_center.append(np.asarray([i[1], i[2], i[3]], dtype="float32"))
            rotation_matrix = camera_data[-1]
            # rotation_matrix=np.matmul(camera_data[-1], rot_matrix)

            print("------------------------------------rotation_matrix_before-------------------------")
            print(rotation_matrix)
            print(camera_data[-1])
            print(camera_data[-1][2][0])
            print(i[1] - camera_data[-1][2][0])

            # i[1]-camera_data[-1:2] 相机圆点位置
            rotation_matrix[:, 0] += (i[1] - camera_data[-1][2][0])
            rotation_matrix[:, 1] += (i[2] - camera_data[-1][2][1])
            rotation_matrix[:, 2] += (i[3] - camera_data[-1][2][2])
            rotation_matrix = np.matmul(rotation_matrix, rot_matrix)
            self.w_gl_item = gl.GLLinePlotItem(
                pos=np.asarray(rotation_matrix),
                width=10,
                color=(1.0, 1.0, 0.0, 1.0),
                mode='line_strip')
            self.graphicsView.addItem(self.w_gl_item)
            camera_data.append(rotation_matrix)
            self.point_slam.setData(
                pos=np.asarray(origin_center),
                width=3,
                color=(1.0, 1.0, 1.0, 1.0),
                mode='line_strip')
        # for i in b:
        #     # 相机位置
        #     origin_center.append(np.asarray([i[1], i[2], i[3]], dtype="float32"))
        #     self.point_slam.setData(
        #         pos=np.asarray(origin_center),
        #         width=3,
        #         color=(1.0, 1.0, 1.0, 1.0),
        #         mode='line_strip')
        #     # 相机姿态 四元组转化为旋转矩阵
        #     rot_matrix = np.asarray([[1 - 2 * i[6] * i[6] - 2 * i[7] * i[7], 2 * i[5] * i[6] + 2 * i[4] * i[7],
        #                               2 * i[5] * i[7] - 2 * i[4] * i[6]],
        #                              [2 * i[5] * i[6] - 2 * i[4] * i[7], 1 - 2 * i[5] * i[5] - 2 * i[7] * i[7],
        #                               2 * i[6] * i[7] + 2 * i[4] * i[5]],
        #                              [2 * i[5] * i[7] + 2 * i[4] * i[6], 2 * i[6] * i[7] - 2 * i[4] * i[5],
        #                               1 - 2 * i[5] * i[5] - 2 * i[6] * i[6]]
        #                              ], dtype="float32")
        #
        #     # R_matrix = np.asarray([[i[1], 0, 0], [0, i[2], 0], [0, 0, i[3]]], dtype="float32")
        #     # t_matrix = np.asarray([i[4], i[5], i[6]], dtype="float32")
        #     t_matrix = np.asarray([i[4], i[5], i[6]], dtype="float32")
        #     print("sp_camera:", self.sp_camera)
        #     # print("R：", R_matrix)
        #     print("t：", t_matrix)
        #     print(origin_center[-1] + t_matrix)
        #
        #     rotation_matrix = camera_data[-1]
        #     # rotation_matrix=np.matmul(camera_data[-1], rot_matrix)
        #
        #     print("------------------------------------rotation_matrix_before-------------------------")
        #     print(rotation_matrix)
        #     print(camera_data[-1])
        #     print(camera_data[-1][2][0])
        #     print(i[1] - camera_data[-1][2][0])
        #
        #     # i[1]-camera_data[-1:2] 相机圆点位置
        #     rotation_matrix[:, 0] += (i[1] - camera_data[-1][2][0])
        #     rotation_matrix[:, 1] += (i[2] - camera_data[-1][2][1])
        #     rotation_matrix[:, 2] += (i[3] - camera_data[-1][2][2])
        #     rotation_matrix = np.matmul(rotation_matrix, rot_matrix)
        #     camera_data.append(rotation_matrix)
        #     print("------------------------------------rotation_matrix-------------------------")
        #     print(rotation_matrix)
        #     # print("------------------------------------origin_center-------------------------")
        #     # print(origin_center)
        #     # print("------------------------------------camera_data-------------------------")
        #     # print(camera_data)
        #
        #     for i in camera_data[1:]:
        #         self.w_gl_item = gl.GLLinePlotItem(
        #             pos=np.asarray(i),
        #             width=10,
        #             color=(1.0, 0, 0.0, 1.0),
        #             mode='line_strip')
        #
        #         self.graphicsView.addItem(self.w_gl_item)
        #     print(len(camera_data))
        #     print(len(origin_center))

    def test_camera_motion(self):
        R_s = np.loadtxt('../data/rotations.txt')
        # print(a)
        R_s = np.reshape(R_s, (-1, 3, 3))
        T_s = np.loadtxt('../data/motions.txt')
        # 相机轨迹
        origin_center = []
        # 初始位置
        origin_center.append(T_s[0])
        # 相机位姿
        camera_data = []
        # 初始姿态
        self.sp_camera = camera.update_camera_position(center=origin_center[0],size=0.6)
        camera_data.append(self.sp_camera)
        # origin_center.append(self.sp_camera[2])
        self.point_slam = gl.GLLinePlotItem(
            pos=origin_center,
            width=3,
            color=(1.0, 1.0, 1.0, 1.0),
            mode='line_strip')

        self.graphicsView.addItem(self.point_slam)
        for i in range(1,len(T_s)):
            # 相机姿态 四元组转化为旋转矩阵
            rot_matrix = R_s[i]

            # R_matrix = np.asarray([[i[1], 0, 0], [0, i[2], 0], [0, 0, i[3]]], dtype="float32")
            # t_matrix = np.asarray([i[4], i[5], i[6]], dtype="float32")
            t_matrix = T_s[i]
            print("sp_camera:", self.sp_camera)
            # print("R：", R_matrix)
            print("t：", t_matrix)
            print(origin_center[-1] + t_matrix)
            # 相机位置
            origin_center.append(t_matrix)
            rotation_matrix = camera_data[0]
            # rotation_matrix=np.matmul(camera_data[-1], rot_matrix)
            rotation_matrix = np.matmul(rotation_matrix, rot_matrix)
            rotation_matrix[:, 0] += (t_matrix[0] - camera_data[0][2][0])
            rotation_matrix[:, 1] += (t_matrix[1] - camera_data[0][2][1])
            rotation_matrix[:, 2] += (t_matrix[2] - camera_data[0][2][2])
            # rotation_matrix = np.matmul(rotation_matrix, rot_matrix)

            camera_data.append(rotation_matrix)
            print("------------------------------------rotation_matrix-------------------------")
            print(rotation_matrix)
            # print("------------------------------------origin_center-------------------------")
            # print(origin_center)
            # print("------------------------------------camera_data-------------------------")
            # print(camera_data)
            self.point_slam.setData(
                pos=np.asarray(origin_center),
                width=3,
                color=(1.0, 1.0, 1.0, 1.0),
                mode='line_strip')
            for i in camera_data[:-1]:
                self.w_gl_item = gl.GLLinePlotItem(
                    pos=np.asarray(i),
                    width=10,
                    color=(1.0, 1.0, 0.0, 1.0),
                    mode='line_strip')

                self.graphicsView.addItem(self.w_gl_item)

    def onUpdateText(self, text):
            cursor = self.process.textCursor()
            cursor.movePosition(QTextCursor.End)
            cursor.insertText(text)
            self.process.setTextCursor(cursor)
            self.process.ensureCursorVisible()

    def pointcloud_mutil_pointcloud_update(self):
        """
        多帧点云数据显示
        点云动态更新
        :return:
        """
        try:
            # print(self.mutil_pointcloud_signNum)
            self.mutil_pointcloud_signNum = self.mutil_pointcloud_signNum + 1  # 一共多少帧点云



        except:
            self.mutil_pointcloud_signNum = 0  # 设置循环播放
    def play_pointclouds_demo(self):
        fileName, filetype = QFileDialog.getOpenFileName(self, "请选择图像：", '.', "All Files(*);;")
        if fileName != '':
            # 读取点云
            pcd = o3d.io.read_point_cloud(fileName)
            # 控制点云数据在中心
            # 获取 Numpy 数组
            print(np.asarray(pcd.points)[:,0].mean())
            print(np.asarray(pcd.points)[:, 1].mean())
            print(np.asarray(pcd.points)[:, 2].mean())

            np_points = np.asarray(pcd.points)
            # np_points[:,0]= np_points[:,0]-np.asarray(pcd.points)[:,0].mean()
            # np_points[:, 1] = np_points[:, 1] - np.asarray(pcd.points)[:, 1].mean()
            # np_points[:, 2] = np_points[:, 2] - np.asarray(pcd.points)[:, 2].mean()
            if pcd.colors:
                np_colors=np.asarray(pcd.colors)
            else:
                np_colors=(1, 0.1, 1, 1)

            self.sp2.setData(pos=np_points, color=np_colors)
            print("opening"+str(fileName)+"")
            # self.graphicsView.addItem(plot)

    def MatrixToImage(self, depth_image_data):
        """
        转化后的深度图转化为能够显示的图像格式
        :param depth_image_data:
        :return: depth_image_array
        """
        depth_image_data = depth_image_data * 255
        depth_image_array = Image.fromarray(depth_image_data.astype(np.uint8))
        return depth_image_array

    def play_muti_video(self,path, speed):
        imagepath= path + '/image/'
        depthpath = path + '/depth/'
        path_list = os.listdir(imagepath+'/')
        path_list_depth=os.listdir(depthpath+'/')
        path_list.sort(key=lambda x: int(x[:-4]))
        print(path_list)
        local_index=0
        for i in range(len(path_list)):
            local_index=i
            img =  cv.imread(imagepath + path_list[i])
            # print(img.shape)
            img2=cv.imread(depthpath + path_list_depth[i], -1)
            depth = cv.split(img2)[0]
            depth[depth > 5000] = 0
            depth = depth / 5000.0000
            img2=np.asarray(depth)
            # print(img)
            image_height, image_width, image_depth = img.shape
            o3d.io.read_image(imagepath + path_list[i])
            QIm = cv.cvtColor(img, cv.COLOR_BGR2RGB)

            QIm = QImage(QIm.data, image_width, image_height, image_width * image_depth, QImage.Format_RGB888)

            self.image_origin.setPixmap(QPixmap.fromImage(QIm))
            self.image_origin.setScaledContents(True)
            image_height, image_width = img2.shape
            # print(img2)

            # QIm = img2#cv.cvtColor(img2, cv.COLOR_BGR2RGB)
            # QIm = QImage(QIm, QIm.shape[1], QIm.shape[0], QImage.Format_Grayscale16)


            QIm =  self.MatrixToImage(img2)
            depth_image = ImageQt.toqpixmap(QIm)
            # QIm = QImage(QIm.data, image_width, QIm.shape[1], QIm.shape[0],  QImage.Format_Grayscale8)

            self.image_depth.setPixmap(depth_image)
            self.image_depth.setScaledContents(True)
            if self.isHidden():
                break
            # 展示多个
            # cv.imshow("mutil_pic", imgs)
            # 等待关闭
            cv.waitKey(speed)
    def selectData(self):
        speed=10
        # imagepath = "./bedroom/image"
        imagepath=QFileDialog.getExistingDirectory()
        print(imagepath)
        # self.play_pointclouds_demo()
        self.play_muti_video(path=imagepath, speed=speed)

#重写键盘事件
class GLViewWidget_1(GLViewWidget):
   pass


if __name__=="__main__":
    import sys

    camera=camera.camera()

    app=QtWidgets.QApplication(sys.argv)

    ui=mywindow()

    ui.show()

    # ui.showMaximized()
    sys.exit(app.exec_())

